#include "communication/include/perception.h"
namespace perception
{
    // namespace lidar_cells
    // {
    Perception::Perception() : Node("perception")
    {
        Init();
    }
    Perception::~Perception() {}
    void Perception::Init()
    {
        RCLCPP_INFO(this->get_logger(), "~~~~~~~~~~~~~lidar perception init~~~~~~~~~~~~~~~~~~");
        InitParams();
        InitSubPub();
        InitAlgorithm();
    }
    void Perception::InitAlgorithm()
    {
        // 读配置文件
        readconfig_ = std::make_shared<ReadConfig>(cell_config_path_, preprocess_config_path_, obj_config_path_);
        readconfig_->ReadCellconfig();
        readconfig_->ReadPreconfig();
        readconfig_->ReadObjconfig();
        std::cout << "cell_config:" << readconfig_->cell_config_.confidence_ << std::endl;
        // 点云预处理
        preprocess_ = std::make_shared<perception::lidar_preprocess::PreProcess>(this);
        lidar_cell_ = std::make_shared<perception::algorithm::LidarCells>(readconfig_->cell_config_, this);
    }
    void Perception::InitSubPub()
    {
        callBackGroup_ = this->create_callback_group(
            rclcpp::CallbackGroupType::
                Reentrant); // 重入（Reentrant：每时刻允许多个线程）
                            // 互斥（MutuallyExclusive：每时刻只允许1个线程）
        options.callback_group = callBackGroup_;

        std::function<void(std::shared_ptr<sensor_msgs::msg::PointCloud2>)> fnc;
        fnc = std::bind(&Perception::CallbackLidar, this, std::placeholders::_1, 0);
        lidar_sub0 = this->create_subscription<sensor_msgs::msg::PointCloud2>(lidar_topic_name0, 10, fnc, options);

        fnc = std::bind(&Perception::CallbackLidar, this, std::placeholders::_1, 1);
        lidar_sub1 = this->create_subscription<sensor_msgs::msg::PointCloud2>(lidar_topic_name1, 10, fnc, options);

        fnc = std::bind(&Perception::CallbackLidar, this, std::placeholders::_1, 2);
        lidar_sub2 = this->create_subscription<sensor_msgs::msg::PointCloud2>(lidar_topic_name2, 10, fnc, options);

        fnc = std::bind(&Perception::CallbackLidar, this, std::placeholders::_1, 3);
        lidar_sub3 = this->create_subscription<sensor_msgs::msg::PointCloud2>(lidar_topic_name3, 10, fnc, options);

        fnc = std::bind(&Perception::CallbackLidar, this, std::placeholders::_1, 4);
        lidar_sub4 = this->create_subscription<sensor_msgs::msg::PointCloud2>(lidar_topic_name4, 10, fnc, options);

        loc_sub = this->create_subscription<msg_interfaces::msg::MatrixProto>(loc_topic, 50, std::bind(&Perception::CallbackLocalization, this, _1), options);
        vs_sub = this->create_subscription<msg_interfaces::msg::MatrixProto>(vs_topic, 50, std::bind(&Perception::CallbackVehicleState, this, _1), options);

        lidar_objs_pub = this->create_publisher<msg_interfaces::msg::MatrixProto>(pub_objs_topic, hz);
        lidar_cells_pub = this->create_publisher<nav_msgs::msg::GridCells>(pub_cells_topic, hz);
        lidar_pre_pub = this->create_publisher<sensor_msgs::msg::PointCloud2>(pub_pre_topic, hz);
        lidar_pre_pub2 = this->create_publisher<sensor_msgs::msg::PointCloud2>(pub_pre_topic2, hz);
        timer_ = this->create_wall_timer(
            100ms, std::bind(&Perception::TimerCallback, this));
    }
    void Perception::InitParams()
    {
        this->declare_parameter<std::string>("lidar_topic0", lidar_topic_name0);
        this->declare_parameter<std::string>("lidar_topic1", lidar_topic_name1);
        this->declare_parameter<std::string>("lidar_topic2", lidar_topic_name2);
        this->declare_parameter<std::string>("lidar_topic3", lidar_topic_name3);
        this->declare_parameter<std::string>("lidar_topic4", lidar_topic_name4);
        this->declare_parameter<std::string>("pub_pre_topic", pub_pre_topic);
        this->declare_parameter<std::string>("pub_pre_topic2", pub_pre_topic2);
        this->declare_parameter<std::string>("pub_cells_topic", pub_cells_topic);
        this->declare_parameter<std::string>("pub_objs_topic", pub_objs_topic);
        this->declare_parameter<std::string>("loc_topic", loc_topic);
        this->declare_parameter<std::string>("vs_topic", vs_topic);
        this->declare_parameter<std::string>("obj_config", obj_config_path_);
        this->declare_parameter<std::string>("cell_config", cell_config_path_);
        this->declare_parameter<std::string>("preprocess_config", preprocess_config_path_);

        this->declare_parameter<int>("hz", hz);
        int max_obj_num;
        this->declare_parameter<int>("max_obj_num", max_obj_num);
        this->get_parameter("lidar_topic0", lidar_topic_name0);
        this->get_parameter("lidar_topic1", lidar_topic_name1);
        this->get_parameter("lidar_topic2", lidar_topic_name2);
        this->get_parameter("lidar_topic3", lidar_topic_name3);
        this->get_parameter("lidar_topic4", lidar_topic_name4);
        this->get_parameter("pub_pre_topic", pub_pre_topic);
        this->get_parameter("pub_pre_topic2", pub_pre_topic2);
        this->get_parameter("pub_cells_topic", pub_cells_topic);
        this->get_parameter("pub_objs_topic", pub_objs_topic);
        this->get_parameter("loc_topic", loc_topic);
        this->get_parameter("vs_topic", vs_topic);
        this->get_parameter("hz", hz);
        this->get_parameter("obj_config", obj_config_path_);
        this->get_parameter("cell_config", cell_config_path_);
        this->get_parameter("preprocess_config", preprocess_config_path_);
        RCLCPP_INFO(this->get_logger(), "vs_topic:%s", vs_topic.c_str());
        RCLCPP_INFO(this->get_logger(), "lidar_topic0:%s", lidar_topic_name0.c_str());
        RCLCPP_INFO(this->get_logger(), "lidar_topic1:%s", lidar_topic_name1.c_str());
        RCLCPP_INFO(this->get_logger(), "lidar_topic2:%s", lidar_topic_name2.c_str());
    }
    void Perception::CallbackLidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg, int lidar_num)
    {
        if (msg->data.empty())
        {
            RCLCPP_WARN(this->get_logger(), " lidar %d has not points cloud!!", lidar_num);
            return;
        }
        DataPool *DP = DataPool::Instance();
        XYZI_PTR points;
        points.reset(new XYZI);
        pcl::fromROSMsg(*msg, *points);
        // 后面所有时间戳用秒
        double time;
        time = double(msg->header.stamp.sec) + double(msg->header.stamp.nanosec) * 1e-9;
        // std::cout << "lidar time:" << time << std::endl;
        // RCLCPP_WARN(this->get_logger(), "  lidar input:%d", points->points.size());
        DP->SetLidarPoints(points, time, lidar_num);
        return;
    }
    void Perception::CallbackLocalization(const msg_interfaces::msg::MatrixProto::SharedPtr msg)
    {
        DataPool *DP = DataPool::Instance();

        std::shared_ptr<std::vector<uint8_t>> vec =
            std::make_shared<std::vector<uint8_t>>();
        vec->resize(msg->len);
        memcpy(vec->data(), msg->buf.data(), msg->len);
        // 反序列化
        localization::Localization loc;
        loc.ParseFromArray(vec->data(), vec->size());
        // 没有时间戳、没有xy方向速度，可能需要加入车辆状态去做
        // 没有时间戳，使用当前的时间临时替代
        DP->main_data_.loc.time = double(rclcpp::Clock().now().seconds()) + double(rclcpp::Clock().now().nanoseconds()) * 1e-9;
        if (loc.loc_pos().loc_status())
        {
            // std::cout << "loc.loc_pos().x():" << loc.loc_pos().x() << " loc.loc_pos().y():" << loc.loc_pos().y() << std::endl;
            DP->main_data_.loc.xg = loc.loc_pos().x();
            DP->main_data_.loc.yg = loc.loc_pos().y();
            DP->main_data_.loc.zg = loc.loc_pos().z();
            DP->main_data_.loc.angle = loc.loc_pos().heading();
        }
        if (loc.loc_pos_gnss().gnss_status())
        {
            DP->main_data_.loc.lat = loc.loc_pos_gnss().latitude();
            DP->main_data_.loc.lon = loc.loc_pos_gnss().longitude();
        }
    }
    void Perception::CallbackVehicleState(const msg_interfaces::msg::MatrixProto::SharedPtr msg)
    {
        DataPool *DP = DataPool::Instance();
        std::shared_ptr<std::vector<uint8_t>> vec =
            std::make_shared<std::vector<uint8_t>>();
        vec->resize(msg->len);
        memcpy(vec->data(), msg->buf.data(), msg->len);
        // 反序列化
        chassis::BusEspInfo loc;
        loc.ParseFromArray(vec->data(), vec->size());
        // std::cout << "loc.esp_veh_spd():" << loc.esp_veh_spd() / 3.6 << " loc.esp_yaw_rate():" << loc.esp_yaw_rate() << std::endl;
        DP->main_data_.loc.speed = loc.esp_veh_spd() / 3.6;
        DP->main_data_.loc.accx = loc.esp_long_accel();
        DP->main_data_.loc.accy = loc.esp_lat_accel();
        DP->main_data_.loc.yawrate = loc.esp_yaw_rate();
    }
    bool Perception::Process()
    {
        DataPool *DP = DataPool::Instance();
        if (!DP->GetLidarPreprocessDataInput())
        {
            RCLCPP_WARN(this->get_logger(), "pre no lidar input:%d", DP->main_data_.lidar_points.point_cloud_ptr_map[0].points->points.size());
            return false;
        }
        auto a = preprocess_->Process(DP->main_data_.preprocess_data_.data_in,
                                      DP->main_data_.preprocess_data_.no_ground_points,
                                      DP->main_data_.preprocess_data_.ground_points, readconfig_->pre_config_);
        // auto a = preprocess_->PreprocessPatchWork(DP->main_data_.lidar_points_cells, data_in, readconfig_->pre_config_);
        // RCLCPP_INFO(this->get_logger(), "ground_points.points num:%d", DP->main_data_.preprocess_data_.ground_points.points->points.size());
        if (a != 0)
        {
            RCLCPP_WARN(this->get_logger(), "preprocess defeat");
            DP->main_data_.lidar_cells_data_.lidar_cells.clear();
            DP->main_data_.lidar_cells_data_.lidar_objs.clear();
            return false;
        }
        if (DP->GetLidarCellDataInput() == 0)
        {
            RCLCPP_WARN(this->get_logger(), "cell no points input");
            return false;
        }
        auto b = lidar_cell_->Process(DP->main_data_.lidar_cells_data_.data_in,
                                      DP->main_data_.loc,
                                      DP->main_data_.lidar_cells_data_.lidar_cells,
                                      DP->main_data_.lidar_cells_data_.lidar_objs);
        if (b != 0)
        {
            RCLCPP_WARN(this->get_logger(), "cluster defeat");
        }
        return true;
    }
    void Perception::PubObjs()
    {

        DataPool *DP = DataPool::Instance();
        // pub objs
        // std::cout << "DP->main_data_.lidar_cells_data_.lidar_objs:" << DP->main_data_.lidar_cells_data_.lidar_objs.size() << std::endl;
        if (!DP->main_data_.lidar_cells_data_.lidar_objs.empty())
        {
            perception::Obstacles objs;
            perception::Header *header;
            header = objs.mutable_header();
            header->set_frame_id("base_link");
            header->set_module_name("lidar_perception");
            header->set_version("v1.0.0");
            auto time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
            header->set_stamp(time.count());
            for (auto obj : DP->main_data_.lidar_cells_data_.lidar_objs)
            {
                auto obj_ = objs.add_obstacles();
                obj_->set_x(obj.x);
                obj_->set_y(obj.y);
                obj_->set_z(obj.z);
                obj_->set_length(obj.length);
                obj_->set_width(obj.width);
                obj_->set_height(obj.height);
                obj_->set_heading(obj.heading * algorithm::rad2deg);
                obj_->set_vx(obj.vx);
                obj_->set_vy(obj.vy);
                obj_->set_age(obj.age);
                obj_->set_confidence(obj.confidence);
                switch (obj.type)
                {
                case 0:
                    obj_->set_classification(perception::ObjectClass::OBJECT_CLASS_CAR);
                    break;
                case 1:
                    obj_->set_classification(perception::ObjectClass::OBJECT_CLASS_TRUCK);
                    break;
                case 2:
                    obj_->set_classification(perception::ObjectClass::OBJECT_CLASS_PEDESTRIAN);
                    break;
                case 3:
                    obj_->set_classification(perception::ObjectClass::OBJECT_CLASS_CYCLIST);
                    break;
                case 5:
                    obj_->set_classification(perception::ObjectClass::OBJECT_CLASS_UNKNOWN);
                    break;
                default:
                    break;
                }
            }
            auto message = msg_interfaces::msg::MatrixProto();
            const size_t data_size = objs.ByteSizeLong();
            std::vector<uint8_t> data(data_size);
            objs.SerializeToArray(data.data(), data_size);
            message.len = data_size;
            message.buf = data;
            lidar_objs_pub->publish(message);
            RCLCPP_INFO(this->get_logger(), "lidarobjs publish ");
        }
    }
    void Perception::Publish()
    {
        DataPool *DP = DataPool::Instance();
        // pub 非地面点云
        if (!DP->main_data_.preprocess_data_.no_ground_points.points->points.empty())
        {
            sensor_msgs::msg::PointCloud2 output;
            pcl::toROSMsg(*DP->main_data_.preprocess_data_.no_ground_points.points, output);
            output.header.frame_id = "base_link";
            lidar_pre_pub->publish(output);
            RCLCPP_INFO(this->get_logger(), "no ground points publish ");
        }
        // pub 地面点云
        // RCLCPP_INFO(this->get_logger(), "ground_points.points num:%d", DP->main_data_.preprocess_data_.ground_points.points->points.size());

        if (!DP->main_data_.preprocess_data_.ground_points.points->points.empty())
        {
            sensor_msgs::msg::PointCloud2 output;
            pcl::toROSMsg(*DP->main_data_.preprocess_data_.ground_points.points, output);

            RCLCPP_INFO(this->get_logger(), "ground_points.points num:%d", DP->main_data_.preprocess_data_.ground_points.points->points.size());

            output.header.frame_id = "base_link";
            lidar_pre_pub2->publish(output);
            RCLCPP_INFO(this->get_logger(), "ground points publish ");
        }
        // pub cells
        if (!DP->main_data_.lidar_cells_data_.lidar_cells.empty())
        {
            nav_msgs::msg::GridCells gcells;
            gcells.cell_width = readconfig_->cell_config_.cell_size_y;
            gcells.cell_height = readconfig_->cell_config_.cell_size_x;
            gcells.header.stamp = rclcpp::Clock().now();
            gcells.header.frame_id = "base_link";
            for (int i = 0; i < DP->main_data_.lidar_cells_data_.lidar_cells.size(); i++)
            {
                for (int j = 0; j < DP->main_data_.lidar_cells_data_.lidar_cells[i].size(); j++)
                {
                    if (DP->main_data_.lidar_cells_data_.lidar_cells[i][j].confidence > 0)
                    {
                        geometry_msgs::msg::Point p;
                        p.x = DP->main_data_.lidar_cells_data_.lidar_cells[i][j].x;
                        p.y = DP->main_data_.lidar_cells_data_.lidar_cells[i][j].y;
                        p.z = DP->main_data_.lidar_cells_data_.lidar_cells[i][j].z_mean;
                        gcells.cells.emplace_back(p);
                    }
                }
            }
            lidar_cells_pub->publish(gcells);
            RCLCPP_INFO(this->get_logger(), "gridcells publish ");
        }
        PubObjs();
        // 清除数据
        DP->main_data_.preprocess_data_.no_ground_points.points->points.clear();
        DP->main_data_.preprocess_data_.ground_points.points->points.clear();
        DP->main_data_.lidar_cells_data_.lidar_cells.clear();
        // DP->main_data_.lidar_cells_data_.lidar_objs.clear();
    }
    void Perception::TimerCallback()
    {
        RCLCPP_INFO(this->get_logger(), "~~~~~~~~~~~~~lidar perception start~~~~~~~~~~~~~~~~~~");
        auto start = std::chrono::system_clock::now();
        if (Process())
            Publish();
        else
            RCLCPP_INFO(this->get_logger(), "lidar detect defeat");
        auto end = std::chrono::system_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
        RCLCPP_INFO(this->get_logger(), "all process cost %f ms", double(duration.count() * 1000) * std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den);
    }

}